% start --- 3robots ----- duration steps ------ plus 2robots ----- duration steps
% ---- drop 1 robots ---- duration steps ---- finish

clc;clear;close all;
savepath = 'Results/Nodefailurerecurrence/3-5-4/';
if ~isfolder(savepath)
    mkdir(savepath);
end
warning('off'); addpath('distributionPlot/','Functions/','Results/','shaded_plots/');
N = 30;
robot_his = cell(N,1); norm_vector = robot_his; H = cell(N,1);
parfor i = 1:N
    [robot_his{i}, norm_vector{i}, H{i}] =  nodefailurerecurrent(i,false);
end
save([savepath,'simdata.mat'],'robot_his','norm_vector','H');

%% dataprocess
clear; close all; clc;
savepath = 'Results/Nodefailurerecurrence/3-5-4/';
load([savepath,'simdata.mat']);

field_limits = [0 100 0 100 0 20];
discretization_num = 50; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

centers = [50 50; 80 80; 20 80; 20 20]; c_index = 1; 
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);

robot = robot_his{1}{1};
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');

MonteCarloNum = size(H,1); t_max = size(H{1},1);
HH = zeros(MonteCarloNum,t_max);
parfor i = 1:MonteCarloNum
    for k = 1:t_max
        disp([num2str(i),':',num2str(k/t_max)])
        robot = robot_his{i}{k};
        H{i}(k) = computeObj_vecform(robot,points_xy,R,mesh_size,r);        
    end
    HH(i,:) = H{i}';
end

save([savepath,'simdata.mat'],'robot_his','norm_vector','H','HH');
%%
% figure()
% distributionPlot(abs(HH(:,1:700)),'histOri','left','color','b','showMM',0,'histOpt',1.1);
% figure()
% distributionPlot(abs(HH(:,701:1400)),'histOri','left','color','r','showMM',0,'histOpt',1.1); 
% figure()
% distributionPlot(abs(HH(:,1401:end)),'histOri','left','color','g','showMM',0,'histOpt',1.1); 
clear; close all; clc;
savepath = 'Results/Nodefailurerecurrence/3-5-4/';
load([savepath,'simdata.mat']);
f1 = figure();     color = [1 0 0; 0 1 0; 0 0 1; 0.5 0.5 0];
set(f1,'position',[1 1 860*2 350*2]); hold on;
x1 = [0 700 700 0];y1 = [5.55 5.55 5.65 5.65]; fill(x1,y1,0.6,'FaceAlpha',0.5); text(300,5.8,'3 robots');
x1 = [700 2*700 2*700 700];y1 = [5.55 5.55 5.65 5.65]; fill(x1,y1,0.7,'FaceAlpha',0.5); text(1000,5.8,'5 robots');
x1 = [2*700 3*700 3*700 2*700];y1 = [5.55 5.55 5.65 5.65]; fill(x1,y1,0.8,'FaceAlpha',0.5);text(1700,5.8,'4 robots');
xlabel('time step'); ylabel('Overall objective function')
text(1070,0.25,'(A)');text(1750,0.25,'(C)');
text(1070,3.5,'(B)'); text(1750,3.5,'(D)');


text(1960,4.4,'around 3.230');
text(1960,4.6,'around 3.246');


plot_distribution_max_min(1:size(HH,2),abs(HH),'Color',[0 0.447 0.741]); axis([1 2101 0 6]); 
x0 = 0:350:2101;set(gca,'XTick',x0);


ax3 = axes(f1,'Position',[0.7 0.2 .15 .35],'Box','on');  HH2 = HH;
HH2(20,:) = [];
plot_distribution_max_min(0:300,abs(HH2(:,1300:1600)),'Color',[0 0.447 0.741]); hold on;
l1 = plot(abs(HH(20,1300:1600)),'Color','r','LineWidth',0.01,'LineStyle',':');
legend(l1, 'Worst case for 1 node failure', 'Location','Northwest'); axis([0 300 0 6.5]);
x0 = 0:100:300;set(ax3,'XTick',x0);set(ax3,'xticklabels',{'1300','1400','1500','1600'});

ax4 = axes(f1,'Position',[0.7 0.63 .15 .15],'Box','on'); 
HH2= HH; HH2(20,:) = [];
distributionPlot(abs(HH2(:,1050:2100)),'histOri','left','color','b','showMM',0,'histOpt',0);
% plot_distribution_max_min(0:200,abs(HH(:,1800:2000)));
% axis([1 201 5.225 5.255]); x0 = 1:50:201;set(ax4,'xtick',x0);set(ax4,'xticklabels',{'1800','1850','1900','1950','2000'});
axis([1,1051 5.15 5.3]); x0 = 1:350:1051;set(ax4,'xtick',x0);set(ax4,'xticklabels',{'1050','1400','1750','2100'});
% text(ax4,550,5.2,'around 3.230');
% text(ax4,550,5.275,'around 3.246');


ax1 = axes('Position',[0.45 0.2 .15 .35],'Box','on'); 
HH1= HH; HH1(9,:) = [];
plot_distribution_max_min(1:900,abs(HH1(:,1:900)),'Color',[0 0.447 0.741]); 
axis([1 900 0 6.5]); x0 = 0:300:900;set(ax1,'XTick',x0);%set(ax1,'xticklabels',{'0','300','600','900'});
hold on; l1 = plot(abs(HH(9,1:900)),'Color','r','LineWidth',0.01,'LineStyle',':'); %l2 = plot(abs(HH(15,1:900)),'g'); l3 = plot(abs(HH(6,1:900)),'k'); hold off;
% legend([l1,l2,l3],'case1','case 2','case 3','Location','NorthWest')
legend([l1],'Worst case for 3 robots','Location','NorthWest')

ax2 = axes(f1,'Position',[0.45 0.63 .15 .15],'Box','on'); 
distributionPlot(abs(HH(:,600:800)),'histOri','left','color','b','showMM',0,'histOpt',0);
axis([1 201 4.8 5.3]); x0 = 1:50:201;set(ax2,'xtick',x0);set(ax2,'xticklabels',{'600','650','700','750','800'});

saveas(f1,[savepath,'Nodefailurerecurren.eps'],'epsc');


function [robot_his, norm_vector, H] =  nodefailurerecurrent(session_num,plot_flag)
% filed limits [x_min x_max y_min y_max z_min z_max]
field_limits = [0 100 0 100 0 20];

% initial robot generation
r_num = 3;                                                  % robot number
state_dim = 3;                                            % state dimensions for each robot
robot = [ rand(r_num,1)*field_limits(2), rand(r_num,1)*field_limits(4), rand(r_num,1)*field_limits(6) ];
robot_scatter_size = 20;

% plot the figure to show robots' realtime positions
if plot_flag
    f = figure('Name','SimultanuouslyCoverage3D'); hold on;
    sc1 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','r'); grid on;
    sc3 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','g');
    axis(field_limits);view(3);
end

% discretizing the x-y plane, 
discretization_num = 60; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

% density calculation
centers = [50 50; 80 80; 20 80; 20 20]; c_index = 1; 
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);

% coverage model
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');

% initial cost
cost = computeObj_vecform(robot,points_xy,R,mesh_size,r);

% simulation time for each session
duration = 700;

% variables container
robot_init = robot;
robot_his = cell(3*duration,1); robot_his{1} = robot; robot_his_count = 1; 
H = zeros(3*duration,1); H(1) =  cost;      % initial cost
norm_vector = cell(3*duration,1); norm_vector_count = 0; 

for k = 1:duration
    robot1 = robot;
    control = zeros(size(robot1));
    norm_v = zeros(r_num,1);
    for i = 1:r_num
        % refine the robot set of multual communication
        s_i = robot(i,:); 
        rel_dis = vecnorm(s_i(1:2) - robot(:,1:2),2,2); 
        r = coverageModel(robot(:,end),sensor_para,'reciprocal');
        robot_i = robot; r1 = rel_dis < r + r(i); r1(rel_dis == 0) = 1;
        robot_i = robot_i .* r1;
        [r,dr_dh] = coverageModel(robot_i(:,end),sensor_para,'reciprocal'); r = r.*r1; dr_dh = dr_dh.*r1;
        % compute the gradient
        delta_u_i = computeGradient(i,robot_i,r,dr_dh,points_xy,mesh_size,R,d_center);
        if norm(delta_u_i,2) == 0
            direction = zeros(size(delta_u_i));
        else
            direction = delta_u_i/norm(delta_u_i,2);   
        end
        norm_v(i) = norm(delta_u_i,2);         
        control(i,:) = 0.1* direction';
    end
    
    robot1= robot1 + control;
    robot = robot1; robot(:,3) = abs(robot(:,3));
    
    % store data
    robot_his{robot_his_count + 1} = robot1; robot_his_count = robot_his_count + 1;
    H(robot_his_count) = computeObj_vecform(robot,points_xy,R,mesh_size,r);
    norm_vector{norm_vector_count+1} = norm_v; norm_vector_count  = norm_vector_count+1;
    if plot_flag
        set(sc3,'XData', robot(:,1), 'YData',robot(:,2),'ZData',abs(robot(:,3)));
        drawnow;
    end
    disp(['Session number',num2str(session_num),' phase: 1 step:' num2str(k),'/',num2str(duration)]);
end
%%
% introducing two new robots
new_r_num = 2;
new_robot =  [ rand(new_r_num,1)*field_limits(2), rand(new_r_num,1)*field_limits(4), rand(new_r_num,1)*field_limits(6) ];
robot = [robot; new_robot];
robot_his{robot_his_count + 1} = robot1; robot_his_count = robot_his_count + 1;
H(robot_his_count) = computeObj_vecform(robot,points_xy,R,mesh_size,r);
r_num = size(robot,1);
if plot_flag
    sc3 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','g');
    sc4 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','b');
end

for k = 1:duration
    robot1 = robot;
    control = zeros(size(robot1));
    norm_v = zeros(r_num,1);
    for i = 1:r_num
        % refine the robot set of multual communication
        s_i = robot(i,:); 
        rel_dis = vecnorm(s_i(1:2) - robot(:,1:2),2,2); 
        r = coverageModel(robot(:,end),sensor_para,'reciprocal');
        robot_i = robot; r1 = rel_dis < r + r(i); r1(rel_dis == 0) = 1;
        robot_i = robot_i .* r1;
        [r,dr_dh] = coverageModel(robot_i(:,end),sensor_para,'reciprocal'); r = r.*r1; dr_dh = dr_dh.*r1;
        % compute the gradient
        delta_u_i = computeGradient(i,robot_i,r,dr_dh,points_xy,mesh_size,R,d_center);
        if norm(delta_u_i,2) == 0
            direction = zeros(size(delta_u_i));
        else
            direction = delta_u_i/norm(delta_u_i,2);   
        end
        norm_v(i) = norm(delta_u_i,2);      
        control(i,:) = 0.1* direction'; 
    end
    robot1= robot1 + control;
    robot = robot1; robot(:,3) = abs(robot(:,3));
    
    % store data
    robot_his{robot_his_count + 1} = robot1; robot_his_count = robot_his_count + 1;
    H(robot_his_count) = computeObj_vecform(robot,points_xy,R,mesh_size,r);
    norm_vector{norm_vector_count+1} = norm_v; norm_vector_count  = norm_vector_count+1;
    if plot_flag
        set(sc4,'XData', robot(:,1), 'YData',robot(:,2),'ZData',abs(robot(:,3)));
        drawnow;
    end
    disp(['Session number',num2str(session_num),' phase: 2 step:' num2str(k),'/',num2str(duration)]);
end

%% reducing one robots
reduceing_num = 1;
new_r_num = randi(5,reduceing_num);
% new_robot =  [ rand(new_r_num,1)*field_limits(2), rand(new_r_num,1)*field_limits(4), rand(new_r_num,1)*field_limits(6) ];
robot(new_r_num,:) = [ ];
robot_his{robot_his_count + 1} = robot1; robot_his_count = robot_his_count + 1;
H(robot_his_count) = computeObj_vecform(robot,points_xy,R,mesh_size,r);
r_num = size(robot,1);

if plot_flag
    sc3 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','b');
    sc4 = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled','k');
end

for k = 1:duration
    robot1 = robot;
    control = zeros(size(robot1));
    norm_v = zeros(r_num,1);
    for i = 1:r_num
        % refine the robot set of multual communication
        s_i = robot(i,:); 
        rel_dis = vecnorm(s_i(1:2) - robot(:,1:2),2,2); 
        r = coverageModel(robot(:,end),sensor_para,'reciprocal');
        robot_i = robot; r1 = rel_dis < r + r(i); r1(rel_dis == 0) = 1;
        robot_i = robot_i .* r1;
        [r,dr_dh] = coverageModel(robot_i(:,end),sensor_para,'reciprocal'); r = r.*r1; dr_dh = dr_dh.*r1;
        % compute the gradient
        delta_u_i = computeGradient(i,robot_i,r,dr_dh,points_xy,mesh_size,R,d_center);
        if norm(delta_u_i,2) == 0
            direction = zeros(size(delta_u_i));
        else
            direction = delta_u_i/norm(delta_u_i,2);   
        end
        norm_v(i) = norm(delta_u_i,2);      
        control(i,:) = 0.1* direction'; 
    end
    robot1= robot1 + control;
    robot = robot1; robot(:,3) = abs(robot(:,3));
    
    % store data
    robot_his{robot_his_count + 1} = robot1; robot_his_count = robot_his_count + 1;
    H(robot_his_count) = computeObj_vecform(robot,points_xy,R,mesh_size,r);
    norm_vector{norm_vector_count+1} = norm_v; norm_vector_count  = norm_vector_count+1;

    if plot_flag
        set(sc4,'XData', robot(:,1), 'YData',robot(:,2),'ZData',abs(robot(:,3)));
        drawnow;
    end
    disp(['Session number',num2str(session_num),' phase: 3 step:' num2str(k),'/',num2str(duration)]);
end

end

